# import picamera2
import cv2 as cv
import numpy as np
from matplotlib import pyplot as plt
from skimage import morphology
# import PCA9685
import os,time,sys
os.environ['DISPLAY'] = ':0'

# cam = picamera2.Picamera2()
# camConfig = cam.create_still_configuration(main={"size": (1920,1080),'format':'BGR888'}, lores={"size": (640, 480)}, display="main")
# camConfig = cam.create_still_configuration()
# cam.configure(camConfig)
# cam.set_controls({"ExposureTime":5000})
# cam.set_controls({"AwbMode":4})
# cam.start()

cap = cv.VideoCapture(0)


def getImage():
    status,image = cap.read()
    return image

def showImage(img):
    plt.imshow(img)
    plt.show()

def showGrayImage(img):
    plt.imshow(img,cmap='gray')
    plt.show()

def isNone(obj):
    return type(obj) == type(None)

def findLargestRectangleContour(img):
    # 检测所有边缘
    contours,hireachy = cv.findContours(img,cv.RETR_CCOMP, cv.CHAIN_APPROX_SIMPLE)

    # 检测周长最大的边缘
    largestContourArea1 = 0
    largestContourArea2 = 0
    largestContourPerimeter = 0
    largestContour1 = None
    largestContour2 = None
    h2 = [-1,-1,-1,-1]
    for i in range(len(contours)):
        c = contours[i]
        h = hireachy[0][i]
        area = cv.contourArea(c)
        perimeter = cv.arcLength(c,True)
        approx = cv.approxPolyDP(c,0.02*perimeter,True)
        if h[3] == -1:
            if area > largestContourArea1 and len(approx) == 4:
                largestContourArea1 = area
                largestContour1 = c
        else:
            # 内轮廓
            if area > largestContourArea2:
                largestContourArea2 = area
                largestContour2 = c
                h2 = h
    if type(largestContour1) == type(None) or type(largestContour2) == type(None):
        return None,None
    else:
        # 检测到最大边缘，进行矩形拟合
        rect1 = cv.minAreaRect(largestContour1)
        approx1 = cv.boxPoints(rect1)
        rect2 = cv.minAreaRect(largestContour2)
        approx2 = cv.boxPoints(rect2)
        return approx1,approx2

def reorderApprox(approx):
    # 给四个点重新排序
    # 既然是顺时针应当按照左下，左上，右上，右下的顺序排列
    xCenter = approx.T[0].mean()
    yCenter = approx.T[1].mean()
    points = [None,None,None,None]
    for point in approx:
        if point[0] < xCenter:
            if point[1] > yCenter:
                #左下
                points[0] = point
            else:
                #左上
                points[1] = point
        else:
            if point[1] < yCenter:
                #右上
                points[2] = point
            else:
                #右下
                points[3] = point
    return points

def searchForPath(img):
    img = cv.cvtColor(img,cv.COLOR_RGB2GRAY)
    _,img = cv.threshold(img,0,255,cv.THRESH_OTSU)
    img = 255 - img
    kernel = np.ones((5,5))
    img = cv.morphologyEx(img,cv.MORPH_OPEN,kernel)
    img = cv.morphologyEx(img,cv.MORPH_CLOSE,kernel)

    imgSkeletone = img.copy()
    imgSkeletone[imgSkeletone == 255] = 1
    imgSkeletone,distance = morphology.medial_axis(imgSkeletone,return_distance=True)
    imgSkeletone = np.uint8(imgSkeletone)
    contours, hierarchy = cv.findContours(imgSkeletone,cv.RETR_CCOMP,cv.CHAIN_APPROX_SIMPLE)
    innerContour = None
    path = None
    for i in range(len(contours)):
        if hierarchy[0][i][3] != -1 and cv.contourArea(contours[i]) > 400:
            innerContour = contours[i]
            break
    if not isNone(innerContour):
        epsilon = 0.0005 * cv.arcLength(innerContour, True)
        path = cv.approxPolyDP(innerContour,epsilon,True)
    path_points = []
    for point in path:
        path_points.append((point[0][0],point[0][1]))
    return path_points


def cannyAndFilter(img):
    imgCanny = cv.Canny(img,32,32)
    kernel = np.ones((3,3))
    imgCannyFiltered = cv.morphologyEx(imgCanny,cv.MORPH_CLOSE,kernel)
    return imgCannyFiltered
    

def searchForGreenLaserPoint(img):
    # 寻找激光点（激光点是红色的）
    imgHSV = cv.cvtColor(img,cv.COLOR_RGB2HSV)
    # lowerRange = np.array([0,0,240]) # 245作为一个阈值
    # upperRange = np.array([180,255,255])
    # 中间的区域全部太亮了导致HSV的色域全部变成0
    # 不如来点歪门邪道，检测边缘区域
    # H: 160 - 180
    # S: 150 - 200
    # V: 210 - 255
    lowerRange = np.array([60,0,245]) # 245作为一个阈值
    upperRange = np.array([120,255,255])
    imgMaskL = cv.inRange(imgHSV,lowerRange,upperRange)
    convKernel = np.ones((4,4),np.uint8) #设置卷积核

    imgFilteredL = cv.morphologyEx(imgMaskL,cv.MORPH_CLOSE,convKernel)
    imgFilteredL = cv.morphologyEx(imgFilteredL,cv.MORPH_OPEN,convKernel)

    imgMomentsL = cv.moments(imgFilteredL,False)
    if imgMomentsL['m00'] == 0:
        return None
    else:
        return imgMomentsL['m10']/imgMomentsL['m00'],imgMomentsL['m01']/imgMomentsL['m00']

def searchForRedLaserPoint(img):    
    diffRGImage = np.uint8(np.array(img[:,:,0],dtype=np.int16)-np.array(img[:,:,1],dtype=np.int16) > 45)
    # diffGRImage = np.uint8(np.array(img[:,:,1],dtype=np.int16)-np.array(img[:,:,0],dtype=np.int16) > 45)
    imgMomentsL = cv.moments(diffRGImage,False)
    
    if imgMomentsL['m00'] == 0:
        return None
    else:
        return imgMomentsL['m10']/imgMomentsL['m00'],imgMomentsL['m01']/imgMomentsL['m00']

# def searchForGreenLaserPoint(img):    
#     # diffRGImage = np.uint8(np.array(img[:,:,0],dtype=np.int16)-np.array(img[:,:,1],dtype=np.int16) > 45)
#     diffGRImage = np.uint8(np.array(img[:,:,1],dtype=np.int16)-np.array(img[:,:,0],dtype=np.int16) > 85)
#     # kernel1 = np.ones((7,7))
#     # diffGRImage = cv.morphologyEx(diffGRImage,cv.MORPH_OPEN,kernel1)
#     imgMomentsL = cv.moments(diffGRImage,False)
    
#     if imgMomentsL['m00'] == 0:
#         return None
#     else:
#         return imgMomentsL['m10']/imgMomentsL['m00'],imgMomentsL['m01']/imgMomentsL['m00']

def searchForRectPoints(img):
    imgGray = cv.cvtColor(img,cv.COLOR_RGB2GRAY)
    imgBin = cv.adaptiveThreshold(imgGray,255,cv.ADAPTIVE_THRESH_GAUSSIAN_C,cv.THRESH_BINARY_INV,101,10)
    # imgBin = cv.inRange(imgBW,(0),(60))
    kernel1 = np.ones((15,15))
    kernel2 = np.ones((5,5))
    imgFiltered = cv.morphologyEx(imgBin,cv.MORPH_CLOSE,kernel1)
    imgFiltered = cv.morphologyEx(imgFiltered,cv.MORPH_OPEN,kernel2)
    # showGrayImage(imgFiltered)
    approx1,approx2 = findLargestRectangleContour(imgFiltered)
    if isNone(approx1) or isNone(approx2):
        return None
    else:
        approx = []
        approx1,approx2 = reorderApprox(approx1),reorderApprox(approx2)
        # print(approx1,approx2)
        for i in range(4):
            approx.append([(approx1[i][0]+approx2[i][0])/2,(approx1[i][1]+approx2[i][1])/2,])
        # print(approx)
        return approx


if __name__ == '__main__':
    fps = 0
    image = getImage()
    path = searchForPath(image)
    while True:
        t0 = time.time()
        image = getImage()
        imageMarked = image.copy()
        
        laserPointRed = searchForRedLaserPoint(image)
        laserPointGreen = searchForGreenLaserPoint(image)
        if not isNone(path):
            for p in path:
                cv.circle(imageMarked,np.int32(p[0]),5,(255,0,0),5)
        if laserPointRed:
            cv.circle(imageMarked,np.int32(laserPointRed),5,(0,0,255),5)
        if laserPointGreen:
            cv.circle(imageMarked,np.int32(laserPointGreen),5,(0,127,127),5)
        # showImage(imageMarked)
        t1 = time.time()
        fps = 0.3/(t1-t0) + fps * 0.7
        imageMarked = cv.putText(imageMarked,"FPS:%0.1f"%(fps),(5,50),cv.FONT_HERSHEY_SIMPLEX,1,(0,0,255),5)
        cv.imshow('video',cv.cvtColor(imageMarked,cv.COLOR_RGB2BGR))
        key = cv.waitKey(1) & 0xff
        if key == ord('q'):
            break
    cv.destroyAllWindows()